import numpy as np
import matplotlib.pyplot as plt
from bezier import normalize_vector, Bezier


class Blend:
    def __init__(self, line1, line2, delta):
        line1_length = np.linalg.norm(np.array(line1[1]) - np.array(line1[0]))
        line2_length = np.linalg.norm(np.array(line2[1]) - np.array(line2[0]))
        delta = np.min((delta, line1_length / 2, line2_length / 2))

        bezier_line1_point1 = np.array(line1[1]) - delta * normalize_vector(np.array(line1[1]) - np.array(line1[0]))
        bezier_line1_point2 = np.array(line1[1])
        bezier_line2_point1 = np.array(line2[0])
        bezier_line2_point2 = np.array(line2[0]) + delta * normalize_vector(np.array(line2[1]) - np.array(line2[0]))
        bezier_line1 = [bezier_line1_point1, bezier_line1_point2]
        bezier_line2 = [bezier_line2_point1, bezier_line2_point2]

        line1_point1 = np.array(line1[0])
        line1_point2 = bezier_line1_point1
        line2_point1 = bezier_line2_point2
        line2_point2 = np.array(line2[1])
        self.line1 = [line1_point1, line1_point2]
        self.line2 = [line2_point1, line2_point2]

        self.bezier = Bezier(self.__cal_parameter(bezier_line1, bezier_line2))

        self.u1 = np.linalg.norm(self.line1[1] - self.line1[0])
        self.u3 = np.linalg.norm(self.line2[1] - self.line2[0])
        self.u2 = self.alpha

        self.u = self.u1 + self.u2 + self.u3

    def interpolate(self, s):
        if s < self.u1 / self.u:
            u_temp = s * self.u / self.u1
            return self.line1[0] + u_temp * (self.line1[1] - self.line1[0])
        elif s < (self.u1 + self.u2) / self.u:
            u_temp = (s - self.u1 / self.u) * self.u / self.u2
            return self.bezier.interpolate(u_temp)
        else:
            u_temp = (s - (self.u1 + self.u2) / self.u) * self.u / self.u3
            return self.line2[0] + u_temp * (self.line2[1] - self.line2[0])

    def __cal_parameter(self, line1, line2):
        t0 = normalize_vector(np.array(line1[1]) - np.array(line1[0]))
        tn = normalize_vector(np.array(line2[1]) - np.array(line2[0]))
        a = 256 - 49 * (np.linalg.norm(t0 + tn) ** 2)
        b = 420 * np.dot((np.array(line2[1]) - np.array(line1[0])), t0 + tn)
        c = -900 * (np.linalg.norm(np.array(line2[1]) - np.array(line1[0])) ** 2)

        self.alpha = np.max(
            ((-b + np.sqrt(b ** 2 - 4 * a * c)) / (2 * a), (-b - np.sqrt(b ** 2 - 4 * a * c)) / (2 * a)))

        p0 = np.array(line1[0])
        p1 = p0 + self.alpha * (1 / 5) * t0
        p2 = 2 * p1 - p0
        p5 = np.array(line2[1])
        p4 = p5 - self.alpha * (1 / 5) * tn
        p3 = 2 * p4 - p5

        return [p0, p1, p2, p3, p4, p5]


if __name__ == "__main__":
    line1 = [(0.0, 0.0), (0.0, 2.0)]
    line2 = [(0.0, 2.0), (2.0, 2.0)]
    delta = 0.5
    blend = Blend(line1, line2, delta)
    num_points = 100
    s_values = np.linspace(0, 1, num_points)
    interpolated_points = []
    for s in s_values:
        interpolated_points.append(tuple(blend.interpolate(s)))
    dx_ds = np.gradient([point[0] for point in interpolated_points], s_values)
    dy_ds = np.gradient([point[1] for point in interpolated_points], s_values)
    d2x_ds2 = np.gradient(dx_ds, s_values)
    d2y_ds2 = np.gradient(dy_ds, s_values)

    # 绘制路径曲线
    plt.subplot(221)
    x_vals = [point[0] for point in interpolated_points]
    y_vals = [point[1] for point in interpolated_points]
    plt.plot(x_vals, y_vals, 'r', label='Interpolated Curve')
    plt.plot([line1[0][0], line1[1][0]], [line1[0][1], line1[1][1]], 'b--', label='Line 1')
    plt.plot([line2[0][0], line2[1][0]], [line2[0][1], line2[1][1]], 'g--', label='Line 2')
    plt.xlabel('x')
    plt.ylabel('y')
    plt.legend()

    # 绘制x和y的位置曲线
    plt.subplot(222)
    plt.plot(s_values, x_vals, label='x')
    plt.plot(s_values, y_vals, label='y')
    plt.xlabel('s')
    plt.ylabel('Position')
    plt.legend()

    # 绘制路径对x和y的偏导数曲线
    plt.subplot(223)
    plt.plot(s_values, dx_ds, label='dx/ds')
    plt.plot(s_values, dy_ds, label='dy/ds')
    plt.xlabel('s')
    plt.ylabel('Derivative')
    plt.legend()

    # 绘制路径对x和y的二阶偏导数曲线
    plt.subplot(224)
    plt.plot(s_values, d2x_ds2, label='d2x/ds2')
    plt.plot(s_values, d2y_ds2, label='d2y/ds2')
    plt.xlabel('s')
    plt.ylabel('Second Derivative')
    plt.legend()

    plt.tight_layout()
    plt.show()
